/*
** ############################################################################
**     Project   : osdee - Operative Systems Design for Embedded Envrionments
**     File      : OS.c
**     Revision  ; 1.0
**     Abstract  :
**         Scheduler Module implementation.
**
**     Copyright : 2014 Fernando Rodriguez - Victor Huidobro, 
**                 Open source with out any responsability.
**     
**     mail      : frr@gmail.com, vhuidobrog@gmail.com
** ############################################################################
*/

#include "OS_TaskM.h"

#define ENTER_PROTECTED_SECTION() boAdquireTCMSemaphore()
#define LEAVE_PROTECTED_SECTION() boReleaseTCMSemaphore()



tsTask_Ctrl_Block*  gPtsTCB;
tsTask_Ctrl_Block*  currentTCB;
tsOS_Stack_Block*   gPtsMainStack;
teOS_Task_Id        teTaskIdZeroOff;
tsBufferState*      gPtsBufferState;
volatile void*      currentTCB_Stack;
teOS_Task_Id        teCurrentRunningTaskId;
BOOL                boTCMSemaphoreAdquired;

/*
** ============================================================================
**     Internal Functions Declaration.
** ============================================================================
*/
BOOL boAdquireTCMSemaphore(void);
BOOL boReleaseTCMSemaphore(void);

/*
** ############################################################################
*/

void OS_Init(const tsOS_Des_Cfg* ptOS_Cfg)
{   
  OS_vTask_Ctrl_Block_Init(ptOS_Cfg); 
  OS_vBuffers_Init();
  (void)ActivateTask(OS_Task_1);

#ifdef  DEBUG_OS_TASK_M
  vfnDebugRoutine();
#endif  
}

/*
** ############################################################################
*/
void OS_vTask_Ctrl_Block_Init(const tsOS_Des_Cfg* ptOS_Cfg){
  UINT8 u8TaskLength;   
  UINT8 u8Index;
  tsTask_Ctrl_Block* ptsTCB;
  tsOS_Task_Cfg const * ptsTask;
  tsOS_Stack_Block* ptsStack;

  u8TaskLength=ptOS_Cfg->u8Task_Number;
  if(0==u8TaskLength){
    return;
  }
  
  u8Index=0;
  ptsTask = ptOS_Cfg->ptsDescriptor;
  teTaskIdZeroOff=ptsTask->tId;
  gPtsTCB = (tsTask_Ctrl_Block *)MemAlloc_NearReserve(u8TaskLength*sizeof(tsTask_Ctrl_Block)); 
  ptsTCB  = gPtsTCB;
  OS_vStackBlock_Init(u8TaskLength);
  ptsStack= gPtsMainStack;
  do{
    ptsTCB->u8Priority = ptsTask->u8Priority;
    ptsTCB->ptTask =  ptsTask++;  
    ptsTCB->tState = OS_Task_Suspended;
    ptsTCB->ptStack= ptsStack++;
    (ptsTCB++)->u8Activations = 0; 
  }while(++u8Index<u8TaskLength);
  
/*****TO DO, Review if this initialization remain valid**/

  currentTCB_Stack=gPtsMainStack;
  currentTCB=gPtsTCB;
  teCurrentRunningTaskId = OS_No_Task;
/**********************************************************/  

  boTCMSemaphoreAdquired = FALSE;
}

/*
** ############################################################################
*/

void OS_vBuffers_Init(void){
  UINT8 u8Index, u8BufferIndex;
  tsBufferState* ptsBuffSt;
  
  gPtsBufferState = (tsBufferState *)MemAlloc_NearReserve(MAX_PRIORITY*sizeof(tsBufferState));
  ptsBuffSt       = gPtsBufferState;
  
  u8Index=0;
    do{
     ptsBuffSt->u8Priority = u8Index;
     ptsBuffSt->u8SetIndex = 0;
     ptsBuffSt->u8GetIndex = 0;       
     ptsBuffSt->ptsStartAddress = (BUFFER_CELL_TYPE)MemAlloc_NearReserve(MAX_BUFFER_DEPTH*sizeof(te_TaskIdRef)); 
     for(u8BufferIndex = 0; u8BufferIndex < MAX_BUFFER_DEPTH; u8BufferIndex++)
       *(ptsBuffSt->ptsStartAddress+u8BufferIndex) = 0xBB00 + u8BufferIndex;
     ptsBuffSt++;
  }while(++u8Index<MAX_PRIORITY);
  
}

/*
** ############################################################################
*/

void OS_vStackBlock_Init(UINT8 u8TaskLength){
    
    gPtsMainStack=(tsOS_Stack_Block *)MemAlloc_NearReserve(u8TaskLength*sizeof(tsOS_Stack_Block)); 

}

#pragma CODE_SEG __NEAR_SEG NON_BANKED

void Os_vWrapperDispatcherFunction(void){
   vDispatcher();
   RestoreContext(); 
  }

#pragma CODE_SEG DEFAULT
  

/*
** ############################################################################
*/

void OS_vSetTaskInBuffer(teOS_Task_Id taskId){
   
  static tsBufferState* ptsBuffSt;
  static tsTask_Ctrl_Block* ptsTCB;
  UINT8 u8Index;
  UINT8 u8Priority;
  
  (void)ENTER_PROTECTED_SECTION();
  ptsTCB      = GET_TCB_REF(taskId);  
  u8Priority  = ptsTCB->ptTask->u8Priority;
  ptsBuffSt   = GET_BUFF_REF(u8Priority);
  if(ptsBuffSt->u8TaskInBuffer<MAX_BUFFER_DEPTH)
  {    
    u8Index     = ptsBuffSt->u8SetIndex;
    ptsBuffSt->ptsStartAddress[u8Index]=taskId;
    ptsBuffSt->u8SetIndex =  ++u8Index % MAX_BUFFER_DEPTH;
    (ptsBuffSt->u8TaskInBuffer)++;  
  }
  (void)LEAVE_PROTECTED_SECTION();
} 


/*
** ############################################################################
*/

teOS_Task_Id OS_tGetTaskInBuffer(void){

  UINT8 u8Index;
  UINT8 u8GetIndex;
  teOS_Task_State tState;
  static tsBufferState* ptsBuffSt;
  static teOS_Task_Id tTaskId;
  ptsBuffSt   = GET_BUFF_REF(MAX_PRIORITY - 1);
  
  (void)ENTER_PROTECTED_SECTION();
  u8Index=MAX_PRIORITY;
  tState = currentTCB->tState;
  do{
     if(ptsBuffSt->u8TaskInBuffer>0){
          if(u8Index>currentTCB->u8Priority || OS_Task_Suspended==tState) 
          { 
            if(OS_Task_Running == tState) 
            {              
              currentTCB->tState=OS_Task_Waiting;
              OS_vSetTaskInBuffer(teCurrentRunningTaskId);           
            }
            u8GetIndex = ptsBuffSt->u8GetIndex;
            (ptsBuffSt->u8TaskInBuffer)--;
            tTaskId=ptsBuffSt->ptsStartAddress[u8GetIndex];
            ptsBuffSt->ptsStartAddress[u8GetIndex] = OS_No_Task; //Clear Memory for debugging purposes        
            ptsBuffSt->u8GetIndex =  (++u8GetIndex) % MAX_BUFFER_DEPTH;
            (void)LEAVE_PROTECTED_SECTION();
            return tTaskId;
          } 
          else 
          {
            (void)LEAVE_PROTECTED_SECTION();
            return OS_No_Task;
          }
            
     }
     else{
        --ptsBuffSt;        
     }
  }while(--u8Index); 
  
  (void)LEAVE_PROTECTED_SECTION();
  return  OS_No_Task;
}



#ifdef  DEBUG_OS_TASK_M

void vfnDebugRoutine(void){

  volatile teOS_Task_Id temp;
  volatile tsTask_Ctrl_Block* testBlock;
  volatile tsBufferState*     testBuffer;  
  
  testBlock=GET_TCB_REF(OS_Task_3);  
  testBuffer=GET_BUFF_REF(OS_Task_6);
  
  OS_vSetTaskInBuffer(OS_Task_1);
  OS_vSetTaskInBuffer(OS_Task_2);
  OS_vSetTaskInBuffer(OS_Task_6);
  
  temp=OS_tGetTaskInBuffer();  
  temp=OS_tGetTaskInBuffer();  
  temp=OS_tGetTaskInBuffer();
  
  OS_vSetTaskInBuffer(OS_Task_3);
  OS_vSetTaskInBuffer(OS_Task_5);
  OS_vSetTaskInBuffer(OS_Task_6);
  temp=OS_tGetTaskInBuffer();    
  OS_vSetTaskInBuffer(OS_Task_1);
  temp=OS_tGetTaskInBuffer();
  temp=OS_tGetTaskInBuffer(); 
  temp=OS_tGetTaskInBuffer();   
  
}
#endif

/*
** ============================================================================
**     Activate Task - Implementation.
** ============================================================================
*/
teE_OS_ERR ActivateTask(teOS_Task_Id taskId)
{
  tsTask_Ctrl_Block* ptsTCB;
  teE_OS_ERR ret = E_OS_OK;
  
  if(taskId < OS_No_Task)
  {
    ptsTCB = GET_TCB_REF(taskId);
    if(ptsTCB->u8Activations < OS_MAX_ACTIVATIONS)
    {
      ptsTCB->u8Activations++;
      ptsTCB->tState = OS_Task_Ready;
      OS_vSetTaskInBuffer(taskId);
      
      //If we were in a pre-emptive system dispatcher should be called here
      //vDispatcher();
    }
    else
    {
      ret = E_OS_LIMIT;
    }
  }
  else
  {
    ret = E_OS_ID;
  }
  
  return ret;
}

/*
** ============================================================================
**     Terminate Task - Implementation.
** ============================================================================
*/
teE_OS_ERR TerminateTask(void)
{
  tsTask_Ctrl_Block* ptsTCB;
  teE_OS_ERR ret = /*boHandlingInterruption ? E_OS_CALLEVEL :*/ E_OS_OK;
  
  if(teCurrentRunningTaskId < OS_No_Task)
  {
    ptsTCB = GET_TCB_REF(teCurrentRunningTaskId);
    ptsTCB->tState = OS_Task_Suspended;
    
    teCurrentRunningTaskId = OS_No_Task;
  }
  else
  {
    ret = E_OS_ID;
  }
  
  return ret;
}

/*
** ============================================================================
**     Dispatcher - Implementation.
** ============================================================================
*/
void vDispatcher(void)
{
  teOS_Task_Id tActiveTaskId = OS_No_Task;
  teOS_Task_Id tLastTaskId   = OS_No_Task;
  tCallbackFunction callBackSwitch;
    
    
    tActiveTaskId = OS_tGetTaskInBuffer();
    if(tActiveTaskId < OS_No_Task)
    {
      currentTCB = GET_TCB_REF(tActiveTaskId);  
      teCurrentRunningTaskId = tActiveTaskId;
      currentTCB_Stack = currentTCB->ptStack;        
     if(OS_Task_Ready == currentTCB->tState)
      {        
        currentTCB->u8Activations--;
        callBackSwitch = currentTCB->ptTask->tCallback;
        asm{
                    ldd  callBackSwitch
                    ldy  currentTCB_Stack
                    std  10,y
                    ldaa 2,sp      
                    staa 1,y 
        };
      }
      currentTCB->tState = OS_Task_Running;
      asm{
                    tpa
                    bita #0x10
                    bne  *+0xD
                    leas 7,sp
                    pula
                    pulx
                    staa -15,sp
                    stx  -14,sp
                    leas -15,sp
                    rtc
                    nop 
         };
    }
}

/*
** ============================================================================
**     GetTaskID - Definition.
** ============================================================================
*/
teE_OS_ERR GetTaskID(te_TaskIdRef pteTaskId)
{
  pteTaskId = &teCurrentRunningTaskId;
  
  return E_OS_OK;
}

/*
** ============================================================================
**     GetTaskState - Definition.
** ============================================================================
*/
teE_OS_ERR GetTaskState(teOS_Task_Id taskId, te_TaskStateRef pteTaskState)
{
  teE_OS_ERR ret = E_OS_OK;
  tsTask_Ctrl_Block* ptsTCB;
  
  pteTaskState = NULL;
  
  if(taskId < OS_No_Task)
  {   
    ptsTCB = GET_TCB_REF(taskId);
    pteTaskState = &ptsTCB->tState;
    ptsTCB->ptTask->tCallback();
  }
  else
  {
    ret = E_OS_ID;
  }
  
  return ret;
}

/*
** ============================================================================
**     TCM Busy - Definition.
** ============================================================================
*/
BOOL boTCMBusy(void)
{
  return boTCMSemaphoreAdquired;
}

/*
** ============================================================================
**     Internal Functions Definitions.
** ============================================================================
*/
BOOL boAdquireTCMSemaphore(void)
{
  BOOL ret = FALSE;
  if(!boTCMSemaphoreAdquired)
  {
    boTCMSemaphoreAdquired = TRUE;
    ret = TRUE;
  }
  
  return ret;
}

BOOL boReleaseTCMSemaphore(void)
{
  BOOL ret = FALSE;
  if(boTCMSemaphoreAdquired)
  {
    boTCMSemaphoreAdquired = FALSE;
    ret = TRUE;
  }
  
  return ret;
}